#include "../src/dormandprince.hh"

#include <iostream> 

namespace
{
  bool
  fuzzy_eq (float a,
            float b)
    {
      return fabs (a - b) <= 1e-4 * (1.0 + fabs (a) + fabs (b));
    }

  struct PowerLaw
    {
      float alpha;
      mutable unsigned int evals;

      PowerLaw (float _alpha) : alpha (_alpha), evals (0) { }

      float
      operator() (float x,
                  float) const
        {
          ++evals;
          return powf (x, alpha);
        }
    };

  void
  test_dormand_prince_once ()
    {
      float alpha;

      do
        {
          alpha = 2.0 * drand48 () - 1.0;
        }
      while (fabsf (alpha) < 0.05);

      float x0;

      do
        {
          x0 = 2.0 * drand48 ();
        }
      while (x0 < 0.05);

      float y0 = 2.0 * drand48 () - 1.0;
      float x1 = x0 + 2.0 * drand48 ();
      PowerLaw power_law (alpha);

      boost::optional<float> integral = 
        flassol::dormand_prince_4_5 (power_law,
                                     x0,
                                     y0,
                                     x1,
                                     1e-5,
                                     0.01,
                                     1.0,
                                     200);

      float plusone = 1.0f + alpha;
      float y1 = y0 + (powf (x1, plusone) - powf (x0, plusone)) / plusone;

      assert (integral || 
              (std::cerr << "integral = false"
                         << " x0 = " << x0
                         << " y0 = " << y0
                         << " x1 = " << x1
                         << " y1 = " << y1
                         << " alpha = " << alpha
                         << " evals = " << power_law.evals
                         << std::endl,
              0));

      assert (power_law.evals <= 7 * ceilf (200.0f / 7.0f)
              || (std::cerr << "power_law.evals = " 
                            << power_law.evals 
                            << std::endl,
                  0));

      assert (fuzzy_eq (*integral, y1) || (
              std::cerr << "*integral = " << *integral
                        << " x0 = " << x0
                        << " y0 = " << y0
                        << " x1 = " << x1
                        << " y1 = " << y1
                        << " alpha = " << alpha
                        << " evals = " << power_law.evals
                        << std::endl,
              0));
    }

  struct Cosine
    {
      float theta;
      mutable unsigned int evals;

      Cosine (float _theta) : theta (_theta), evals (0) { }

      float
      operator() (float x,
                  float) const
        {
          ++evals;
          return cosf (theta * x);
        }
    };

  void
  test_dormand_prince_mega_once ()
    {
      float theta;

      do
        {
          theta = 2.0 * drand48 () - 1.0;
        }
      while (fabsf (theta) < 0.01);

      float x0 = 2.0 * drand48 () - 1.0;
      float y0 = 2.0 * drand48 () - 1.0;
      float x1 = x0 + 2.0 * drand48 ();
      Cosine cosine (theta);

      boost::optional<float> integral = 
        flassol::dormand_prince_4_5 (cosine,
                                     x0,
                                     y0,
                                     x1,
                                     1e-5,
                                     0.01,
                                     1.0,
                                     100);

      float y1 = y0 + (sinf (theta * x1) - sinf (theta * x0)) / theta;

      assert (integral || 
              (std::cerr << "integral = false"
                         << " x0 = " << x0
                         << " y0 = " << y0
                         << " x1 = " << x1
                         << " y1 = " << y1
                         << " theta = " << theta
                         << " evals = " << cosine.evals
                         << std::endl,
              0));

      assert (cosine.evals <= 7 * ceilf (200.0f / 7.0f)
              || (std::cerr << "cosine.evals = " 
                            << cosine.evals 
                            << std::endl,
                  0));

      assert (fuzzy_eq (*integral, y1) || (
              std::cerr << "*integral = " << *integral
                        << " x0 = " << x0
                        << " y0 = " << y0
                        << " x1 = " << x1
                        << " y1 = " << y1
                        << " theta = " << theta
                        << " evals = " << cosine.evals
                        << std::endl,
              0));
    }

  void
  test_dormand_prince ()
    {
      for (unsigned int n = 0; n < 1000; ++n)
        {
          test_dormand_prince_once ();
          test_dormand_prince_mega_once ();
        }
    }
}

int
main (void)
{
  srand48 (69);
  test_dormand_prince ();

  return 0;
}


